%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%ProjBug%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
% Creator Miguel Moises Serrano
%         786 333 9195
%         Georgia Institute of Technology
%         
% Description:
%     Tool using common robotics and comp. vision techniques for
%     constructing limbs, bodies, and projection cameras for the sake 
%     of biomechanical studies
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function pb = projBug()

pb.Body=@Body;
pb.limb=@forwardKinLimb;
pb.vert=@forwardKinVertabre;
pb.proj=@MultiCamProjection;
pb.viewer=@KinViewer;
pb.vsh=@KinViewerShade;

pb.getPointCloud=@pointCloud1;
pb.getPointCloudFromState=@pointCloudS1;

% % % % % % % % % % % % % % Autonomous Model. Generates point cloud
% % % % % % % % % % % % % % given a state space coordinate % % % % % % %

function [lpts0 depInfo] = pointCloudS1( xhat )
    % xhat: a state space position
    % this function defines a way to generate the observed point cloud as a function of
    % the unobservable underlying state xhat
    % outputs: lpts0- the point cloud of points on the bug body
    % depInfo: a list of indices of states with which
    % are coupled / co-depends on eachother.
    % this is used to de-couple during optimization so that computation is
    % tractable.
    
    % the current implementation has 2 parameters for each leg as the time
    % varying part
    % the others are currently constant
    
    % current dynamics model: legs are independent, two variables per leg
    % are time-varying
    % thus there are 8 disjoint coupled sets of variables
    depInfo = cell(8,1);
    depInfo{1} = [1 2];
    depInfo{2} = [3 4];
    depInfo{3} = [5 6];
    depInfo{4} = [7 8];
    depInfo{5} = [9 10];
    depInfo{6} = [11 12];
    depInfo{7} = [13 14];
    depInfo{8} = [15 16];
        
    l=10;
    d=l/5;

    if( length(xhat) ~= 16 )
        error('improper size of xhat state vector in pointCloudS1()');
    end
    
    % legs
    lpts0=[0 0 0].';
    p=[10 10 10].';
    p1=.15*[100 100 100].';
    kk = 1;
    for m= 1:2
        for n= 1:4
            % arg 1: the angles (x y z) shoulder, elbow 1, elbow 2
            % arg 2: lengths
            [lpts,lptsN]=pb.limb(pi/180*[(-1)^m*36*n -30 0 xhat(kk) xhat(kk+1)],[10 20 30]/2,'two',[ 2*(-1)^(m+1)*sin(pi*(d*n)/l) l/2-d*n 0],16,.5);
            % lptsN: the "expanded" dense cloud of points
            % lpts: the vertices of the current joint
            lpts0=[lpts0, lptsN];
            % lpts0: the collection of all dense points (visual representation
            % of the bug)
            
            kk = kk + 2;
        end
    end

    % body
    lptsN=pb.Body([eye(3) [0 -l/2 0].'; 0 0 0 1],[eye(3) [0 l/2 0].'; 0 0 0 1],1,.5,2);
    lpts0=[lpts0, lptsN];
    lptsN=pb.Body([rx(pi/8) [0 0 0].'; 0 0 0 1],[eye(3) [0 l*2 0].'; 0 0 0 1],.5,.5,2);
    lpts0=[lpts0, lptsN];
    lptsN=pb.Body([rx(7*pi/8) [0 -3*l/8 0].'; 0 0 0 1],[eye(3) [0 -3*l/4 0].'; 0 0 0 1],.75,2.5,1);
    lpts0=[lpts0, lptsN]; 
        
end

% % % % % % % % % % % % % % Reference/True Model. Generates true state
% % % % % % % % % % % % % % and point cloud from given time t % % % % % % % % % % %  
    function [lpts0 xhat] = pointCloud1( t )
    % t: a time parameter
    % this function defines a way to generate the state as a function of
    % time
    % outputs: lpts0- the point cloud of points on the bug body
    % xhat: the vector of time-varying states
    
    % the current implementation has 2 parameters for each leg as the time
    % varying part
    % the others are currently constant
        
    l=10;
    d=l/5;

    % legs
    lpts0=[0 0 0].';
    p=[10 10 10].';
    p1=.15*[100 100 100].';
    xhat = [];
    for m= 1:2
        for n= 1:4
           
            xhat = [xhat; 90+20*sin(t+m*n*0.1); -130+20*sin(t+m*n*0.1) ];

        end
    end
    
    lpts0 = pointCloudS1( xhat );

    % body
%     lptsN=pb.Body([eye(3) [0 -l/2 0].'; 0 0 0 1],[eye(3) [0 l/2 0].'; 0 0 0 1],1,0.5,2);
%     lpts0=[lpts0, lptsN];
%     lptsN=pb.Body([rx(pi/8) [0 0 0].'; 0 0 0 1],[eye(3) [0 l*2 0].'; 0 0 0 1],.5,0.5,2);
%     lpts0=[lpts0, lptsN];
%     lptsN=pb.Body([rx(7*pi/8) [0 -3*l/8 0].'; 0 0 0 1],[eye(3) [0 -3*l/4 0].'; 0 0 0 1],.75,0.5,1);
%     lpts0=[lpts0, lptsN]; 
        
    end

% % % % % % % % % % % % % % Body and Form Creator % % % % % % % % % % %  
    function [lpts]=Body(g1,g2,f,q,delta)
%       bodyO body orientation [x y z]
%       l legth of body
%       d is divisions
%       q additional division factor
%       f thinness factor
    if nargin<1
        g1=[rx(-pi/2), [ 0 0 0 ].'; 0 0 0 1];
    end
    if nargin<2
        g2=[eye(3), [0 6 0 ].'; 0 0 0 1];
    end
        d=sqrt((g2(1,4)-g1(1,4))^2 +(g2(2,4)-g1(2,4))^2 +(g2(3,4)-g1(3,4))^2);
        
    if nargin<5
        delta=d;
    end
        lpts=[0 0 0]';
        for n1=0:1/q:d
            for n2=0:1/q:d               
               r1=g1;
               r2=[ry(2*pi*n2/(d)),[0 n1  0 ]';0 0 0 1];
               r3=[eye(3),[ (delta/f)*sin(pi*(n1)/d),0,0]'; 0 0 0 1];
               g=r1*r2*r3; 
               lpts = [lpts, g(1:3,4)];
            end
        end
        
    end
% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 

% % % % % % % % % % % % % % % % Limb Creator % % % % % % % % % % % % % 
    function [lpts,lpts0]=forwardKinLimb(alpha,l,type,body,f,q)
%         there are two types considered, with feet without feet, type one
%         and type two respectively
%         body stands for the body location of the appendage vector of
%         lengths
%         lpts0 has all the points (volume), lpts is only the end points

    if nargin<1
        l=10*[1 1 1];
        alpha=[0 0 0 0 0];
        type='two';
        body=[0 0 0];
        q=1;
    end
    lpts=body.';
    lpts0=body.';
    r=alpha;
    ge=[eye(3),body.'; 0 0 0 1];
    [n1,n2]=size(l);
%     construct the leg attachments



            ge=ge*[rz(r(1)), [0,0,0].';0 0 0 1];
            ge=ge*[rx(r(2)), [0,0,0].';0 0 0 1];
            ge=ge*[ry(r(3)), [0,0,0].';0 0 0 1];  
            if n2>n1
                n=n2;
            else
                n=n1;
            end
            for m=1:n-1
                gr=ge;
                ge=ge*[rx(r(m+3)), [0,l(m),0].';0 0 0 1];     
                lptsN=Body(gr,ge,m*f,q);       
                lpts = [lpts, ge(1:3,4)];
                lpts0= [lpts0,lptsN];
            end
            switch(type)
                case 'one'
                    ge=ge*[rz(r(3+m)), [0,0,0].';0 0 0 1];
                    ge=ge*[rx(r(4+m)), [0,0,0].';0 0 0 1];
                    ge=ge*[ry(r(5+m)), [0,0,0].';0 0 0 1];
                    gr=ge;
                    ge=ge*[eye(3), [0,l(end),0].';0 0 0 1];
                    lptsN=Body(gr,ge,f*n,q); 
                    lpts = [lpts, ge(1:3,4)];                     
                    lpts0= [lpts0,lptsN];
                case 'two'                    
                    gr=ge;
                    ge=ge*[eye(3), [0,l(end),0].';0 0 0 1];
                    lptsN=Body(gr,ge,f*n,q); 
                    lpts = [lpts, ge(1:3,4)];                     
                    lpts0= [lpts0,lptsN];
            end
    end
% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %

% % % % % % % % % % % % % % % % Spine Creator % % % % % % % % % % % % % 
    function [ge,lpts] = forwardKinVertabre(l,alpha)
        % if size of l is  n, then alpha must be n-1
        % l is matrix of lengths for the spine with alpha as a matrix or joint angles
        % alpha is an nxm matrix, ever row is the respective x,y,z
        % orientation
        if nargin < 1
            l=ones(1,20);
            alpha=zeros(19,3);
        end
        
        lpts = [0;0;0];        
        [n1,n2]=size(l);
        ge=[eye(3),[0 0 0].'; 0 0 0 1];
        for p1= 1: n2
            if p1==1
                r=eye(3);
            else
                r=alpha(p1-1,:);
            end
            ge=ge*[eye(3), [0,0,l(p1)].';0 0 0 1];
            ge=ge*[rz(r(1)), [0,0,0].';0 0 0 1];
            ge=ge*[rx(r(2)), [0,0,0].';0 0 0 1];
            ge=ge*[ry(r(3)), [0,0,0].';0 0 0 1];  
            lpts = [lpts, ge(1:3,4)];
        end

        KinViewer(lpts);
    end
% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 

% % % % % % % % % % % % % % % % Cam Creator % % % % % % % % % % % % % %
    function projs = MultiCamProjection(lpts,camNum,g)
        if nargin<2
            camNum=1;
            alphas= pi/180* [45 180 45 ]';
            r=ry(-pi/2)*rz(pi/2);
            d=[20 0 0]';
            g=[r,d;0 0 0 1];
        end
%        camNum is the number of cameras
%        alphas are the respective angles for the camera orientation (R^3 input)
%        locs are the repective locations for the camera orientation (R^3 input)
%        future addition, respective t matrices
        [n1,n2]=size(lpts);
        
        if n1==3
        projs=zeros(n1,n2,camNum);
        else
        projs=zeros(n1-1,n2,camNum);
        end
        figure();
        for n=1:camNum
           projs(:,:,n) =  camera(lpts,g(:,:,n));     
           subplot(camNum^.5,camNum^.5,n);
           plot(projs(1,:,n),projs(2,:,n),'k.');
           axis([0 400 0 800]);
          
        end
    end
% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 

% % % % % % % % % % % % % % % % View Creator % % % % % % % % % % % % % %
    function KinViewer(lpts,n)
figure(n);
        hold on;
        
        axfact = 100;
        plot3(axfact*[-1 1],[0 0],[0 0],'b-.');
        plot3([0 0], axfact*[-1 1],[0 0],'b-.');
        plot3([0 0], [0 0], axfact*[-1 1],'b-.');
        plot3(lpts(1,2:end-1), lpts(2,2:end-1), lpts(3,2:end-1), 'k.','MarkerSize',10);
        hold off;

        axis([-20, 20, -20, 20, -20, 20]);

    end
% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 

% % % % % % % % % % % % % View with shade Creator % % % % % % % % % % % 
    function shade = KinViewerShade(lpts,n,p)
%       p is the light source
        figure(n);
        hold on;
        [n1,n2]=size(lpts);
        
        for m=1:n2
            shade(1,m)=norm([lpts(1,m)-p(1);lpts(2,m)-p(2);lpts(3,m)-p(3)]);            
        end
        shade=1-shade/max(max(abs(shade)));
       

        
        axfact = 100;
        plot3(axfact*[-1 1],[0 0],[0 0],'b-.');
        plot3([0 0], axfact*[-1 1],[0 0],'b-.');
        plot3([0 0], [0 0], axfact*[-1 1],'b-.');
        hold on
    for m=2:n2
        plot3(lpts(1,m), lpts(2,m), lpts(3,m), '.','Color',shade(m)*[1 2 1]/norm([1  2 1]),'MarkerSize',10);     
    end        
        hold off;

        axis([-20, 20, -20, 20, -20, 20]);

    end
% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %

end
%============================= Cam Function =============================
function Cmr = camera(mat,g)
%   loc is the location of the cam respective of the origin
%   alpha is the 3 dof angles for the cam's orientation
%   mat is the set of points in the 3dimensional world hence [x;y;z]
     f=20*10^-3;
     mat = [mat; ones(1,size(mat,2))];
     t=[[f/.005 0 0;  0 f/.010 0 ; 0 0 1],zeros(3,1)];
% research t matrix
if nargin<2
     r=[cos(pi) 0 -sin(pi); 0 1 0; sin(pi) 0 cos(pi)];
     d=[0 0 10].';
     g=[r,d; 0 0 0 1];
elseif nargin<1
     r=[cos(pi) 0 -sin(pi); 0 1 0; sin(pi) 0 cos(pi)];
     d=[0 0 10].';
     g=[r,d; 0 0 0 1];
end
    Cmr=t*g*mat;    
%     Cmr(1,:) = Cmr(1,:)/.01+200;
%     Cmr(2,:) = Cmr(2,:)/.005+400;
    Cmr(1,:) = Cmr(1,:)./Cmr(3,:)+400/2;
    Cmr(2,:) = Cmr(2,:)./Cmr(3,:)+800/2;
    Cmr(3,:) = ones(1,size(Cmr,2));
end

%============================ Helper Functions ===========================
function re=rx(alpha)
re=[1 0 0; 0 cos(alpha) -sin(alpha) ; 0 sin(alpha) cos(alpha)];
end
function re=ry(alpha)
re=[cos(alpha) 0 sin(alpha); 0 1 0  ; -sin(alpha) 0 cos(alpha)];
end
function re=rz(alpha1)
re=[ cos(alpha1) -sin(alpha1) 0; sin(alpha1) cos(alpha1) 0; 0 0 1];
end
function t=inver(g)

r=(g((1:3),(1:3)))^-1;
d=-((g((1:3),(1:3)))^-1)*g((1:3),4);
t=[r d; 0 0 0 1];

end